#ifndef _ROBOTCONTROLLER_
#define _ROBOTCONTROLLER_

#include <Aria.h>
#include <string>
#include "finishingGroupAction.h"
#include "targetNearGroupAction.h"
#include "targetFarGroupAction.h"
#include "ScanningGroupAction.h"
#include "VisioneController.h"
#include "math.h"
#define PI 3.14159265
using namespace std;

class RobotController{

	private:
		enum State{
			INIT,
			SCANNING,
			TARGETFAR,
			TARGETNEAR,
			ASSESMENT,
			ALIGN,
			FINISHING,
		};

		FinishingGroupAction* finishing;
		TargetFarGroupAction* targetfar;
		TargetNearGroupAction* targetnear;
		ScanningGroupAction* scanning;
		ArPose *goalpos;
		double ball_degree;
		ArRobot* robot;
		State myState,myOldState;
		int threshold,th_ball;
		unsigned short thPort;
		pthread_t thread;
		VisioneController *controllerV;
		int angolo;
		bool align;
		double beta;
		void dowork();
		void dowork_final();
		static void* start(void *obj){reinterpret_cast<RobotController*>(obj)->dowork();}
	public:
		RobotController(ArRobot*);
		~RobotController();
		void stop();
		void run();
};

#endif
